/**
 ******************************************************************************
 * @file           : bsp_mpu6050.c
 * @brief          : MPU6050 Six-Axis Sensor Driver Implementation (STM32F4xx)
 ******************************************************************************
 * @attention
 *
 * MPU6050驱动实现文件
 * 通过I2C接口读取加速度和角速度数据
 *
 ******************************************************************************
 */

/* Includes ------------------------------------------------------------------*/
#include "./Devices/MPU6050/bsp_mpu6050.h"
#include "./Devices/MPU6050/bsp_mpu6050_config.h"
#include "./BSP/IIC/bsp_iic.h"

/* Private includes ----------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/

/* MPU6050寄存器地址 */
#define MPU6050_REG_ACCEL_X_H       0x3B
#define MPU6050_REG_ACCEL_X_L       0x3C
#define MPU6050_REG_ACCEL_Y_H       0x3D
#define MPU6050_REG_ACCEL_Y_L       0x3E
#define MPU6050_REG_ACCEL_Z_H       0x3F
#define MPU6050_REG_ACCEL_Z_L       0x40
#define MPU6050_REG_TEMP_H          0x41
#define MPU6050_REG_TEMP_L          0x42
#define MPU6050_REG_GYRO_X_H        0x43
#define MPU6050_REG_GYRO_X_L        0x44
#define MPU6050_REG_GYRO_Y_H        0x45
#define MPU6050_REG_GYRO_Y_L        0x46
#define MPU6050_REG_GYRO_Z_H        0x47
#define MPU6050_REG_GYRO_Z_L        0x48
#define MPU6050_REG_PWR_MGMT_1      0x6B
#define MPU6050_REG_WHO_AM_I        0x75

/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/

/**
 * @brief       MPU6050初始化
 *
 * @param       无
 *
 * @retval      0=成功，1=失败
 */
uint8_t mpu6050_init(void)
{
    uint8_t id = 0;

    /* 检查设备是否存在 */
    if (iic_read_reg(MPU6050_I2C_ADDR, MPU6050_REG_WHO_AM_I, &id) != 0)
    {
        log_e("MPU6050 not found");
        return 1;
    }

    if (id != 0x68)
    {
        log_e("MPU6050 ID error: 0x%02X", id);
        return 1;
    }

    /* 唤醒MPU6050 */
    if (iic_write_reg(MPU6050_I2C_ADDR, MPU6050_REG_PWR_MGMT_1, 0x00) != 0)
    {
        log_e("MPU6050 wakeup failed");
        return 1;
    }

    log_i("MPU6050 initialized successfully");
    return 0;
}

/**
 * @brief       MPU6050去初始化
 *
 * @param       无
 *
 * @retval      无
 */
void mpu6050_deinit(void)
{
    /* 进入睡眠模式 */
    iic_write_reg(MPU6050_I2C_ADDR, MPU6050_REG_PWR_MGMT_1, 0x40);
}

/**
 * @brief       读取MPU6050所有数据
 *
 * @param[out]  data 数据结构体
 *
 * @retval      0=成功，1=失败
 */
uint8_t mpu6050_read_data(MPU6050_Data_t *data)
{
    uint8_t buf[14];

    if (data == NULL)
    {
        return 1;
    }

    /* 一次读取14字节数据 */
    if (iic_read_bytes(MPU6050_I2C_ADDR, MPU6050_REG_ACCEL_X_H, buf, 14) != 0)
    {
        return 1;
    }

    /* 解析加速度数据 */
    data->accel_x = (int16_t)((buf[0] << 8) | buf[1]);
    data->accel_y = (int16_t)((buf[2] << 8) | buf[3]);
    data->accel_z = (int16_t)((buf[4] << 8) | buf[5]);

    /* 解析温度数据 */
    data->temperature = (int16_t)((buf[6] << 8) | buf[7]);

    /* 解析陀螺仪数据 */
    data->gyro_x = (int16_t)((buf[8] << 8) | buf[9]);
    data->gyro_y = (int16_t)((buf[10] << 8) | buf[11]);
    data->gyro_z = (int16_t)((buf[12] << 8) | buf[13]);

    return 0;
}

/**
 * @brief       读取加速度数据
 *
 * @param[out]  x X轴加速度
 * @param[out]  y Y轴加速度
 * @param[out]  z Z轴加速度
 *
 * @retval      0=成功，1=失败
 */
uint8_t mpu6050_read_accel(int16_t *x, int16_t *y, int16_t *z)
{
    uint8_t buf[6];

    if (iic_read_bytes(MPU6050_I2C_ADDR, MPU6050_REG_ACCEL_X_H, buf, 6) != 0)
    {
        return 1;
    }

    if (x != NULL)
    {
        *x = (int16_t)((buf[0] << 8) | buf[1]);
    }

    if (y != NULL)
    {
        *y = (int16_t)((buf[2] << 8) | buf[3]);
    }

    if (z != NULL)
    {
        *z = (int16_t)((buf[4] << 8) | buf[5]);
    }

    return 0;
}

/**
 * @brief       读取陀螺仪数据
 *
 * @param[out]  x X轴角速度
 * @param[out]  y Y轴角速度
 * @param[out]  z Z轴角速度
 *
 * @retval      0=成功，1=失败
 */
uint8_t mpu6050_read_gyro(int16_t *x, int16_t *y, int16_t *z)
{
    uint8_t buf[6];

    if (iic_read_bytes(MPU6050_I2C_ADDR, MPU6050_REG_GYRO_X_H, buf, 6) != 0)
    {
        return 1;
    }

    if (x != NULL)
    {
        *x = (int16_t)((buf[0] << 8) | buf[1]);
    }

    if (y != NULL)
    {
        *y = (int16_t)((buf[2] << 8) | buf[3]);
    }

    if (z != NULL)
    {
        *z = (int16_t)((buf[4] << 8) | buf[5]);
    }

    return 0;
}

/**
 * @brief       读取温度数据
 *
 * @param[out]  temp 温度（原始值）
 *
 * @retval      0=成功，1=失败
 */
uint8_t mpu6050_read_temp(int16_t *temp)
{
    uint8_t buf[2];

    if (temp == NULL)
    {
        return 1;
    }

    if (iic_read_bytes(MPU6050_I2C_ADDR, MPU6050_REG_TEMP_H, buf, 2) != 0)
    {
        return 1;
    }

    *temp = (int16_t)((buf[0] << 8) | buf[1]);

    return 0;
}

/**
 * @brief       设置加速度范围
 *
 * @param[in]   range 范围（0=±2g, 1=±4g, 2=±8g, 3=±16g）
 *
 * @retval      0=成功，1=失败
 */
uint8_t mpu6050_set_accel_range(uint8_t range)
{
    if (range > 3)
    {
        return 1;
    }

    return iic_write_reg(MPU6050_I2C_ADDR, 0x1C, (range << 3));
}

/**
 * @brief       设置陀螺仪范围
 *
 * @param[in]   range 范围（0=±250°/s, 1=±500°/s, 2=±1000°/s, 3=±2000°/s）
 *
 * @retval      0=成功，1=失败
 */
uint8_t mpu6050_set_gyro_range(uint8_t range)
{
    if (range > 3)
    {
        return 1;
    }

    return iic_write_reg(MPU6050_I2C_ADDR, 0x1B, (range << 3));
}
